/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.model;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.util.Point2D;
import com.team1160.breakaway.util.Point2DDouble;
import edu.wpi.first.wpilibj.camera.AxisCamera;


/*This class holds variables representing all low level states of the robot,
 * i.e. motor values, solenoid values, etc
 *
 * This is set by the low-level classes, the Kicker,Dribbler, and DriveTrain.
 *
 * its values are used by RobotStateReader
 *
 * @author Team 1160 Firebird Robotics (saketh kasibatla)
 */
public class Model{
    //final instance fields
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Default instance of the Model class
     */
    protected static final Model DEFAULT_MODEL = new Model();

    //protected instance fields
    ////////////////////////////////////////////////////////////////////////////

    //drivetrain
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Drivetrain motor value
     */
    protected double m_dt_frontLeft, m_dt_frontRight, m_dt_backLeft, m_dt_backRight;

    //dribbler
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Dribbler motor value
     */
    protected double m_dr;

    //kicker
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Solenoid values for fire piston
     */
    protected boolean s_k_fire_front,s_k_fire_back;

    /**
     * Solenoid values for latch piston
     */
    protected boolean s_k_latch_front, s_k_latch_back;

    //analog input (gyro,encoder,accel)
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Gyrometer angle
     */
    protected double g_angle;

    /**
     * Rate of accelerometer
     */
    protected double a_rate;


    protected Point2D targetCenter;
    
    protected Point2D cameraDimension;


    protected Point2DDouble targetCenterNormalized;
//    protected int targetOrientation;

    /**
     * This value is 0 when the robot is too far to the left,
     * 2 if the robot is too far to the right, and 1 if the
     * robot is on target, and -1 if the robot can't see a target.
     */
    //TODO mango write a method to update this.
    //protected byte robotOnTarget;


    //constructor
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Limits the number of instances to one
     */
    private Model(){
        reset();
    }

    //accessor methods
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Gets the default instance of the model class.
     * @return The default model instance
     */
    public static Model getInstance(){
        return DEFAULT_MODEL;
    }

    /**
     * Gets the value of a drivetrain motor given the ID
     * @param motorID the ID of the motor values in the constants class
     * @return the value of hte motor
     * @throws IllegalArgumentException thrown if the value is not one fo the constants.
     * @see Constants
     */
    public double getDriveTrainMotorValue(byte motorID)throws IllegalArgumentException{
        switch(motorID){
            case Constants.ID_DRIVETRAIN_LEFT_FRONT:
                return this.m_dt_frontLeft;
            case Constants.ID_DRIVETRAIN_RIGHT_FRONT:
                return this.m_dt_frontRight;
            case Constants.ID_DRIVETRAIN_LEFT_BACK:
                return this.m_dt_backLeft;
            case Constants.ID_DRIVETRAIN_RIGHT_BACK:
                return this.m_dt_backRight;
            default:
                throw new IllegalArgumentException("ERROR: Illegal motor ID, use the values in the Constants class.");
        }
    }

    /**
     * Gets the value of the dribbler motor
     * @return the value of the motor
     */
    public double getDribblerMotorValue(){
        return this.m_dr;
    }

    /**
     * Gets the value of a solenoid given the ID.
     * @param solenoidID the ID of the solenoid. Use values from the constants class.
     * @return the value
     * @throws IllegalArgumentException if the ID isn't in the constants class
     * @see Constants
     */
    public boolean getSolenoidValue(byte solenoidID) throws IllegalArgumentException{
        switch(solenoidID){
            case Constants.ID_KICKER_SOLENOID_FIRE_FRONT:
                return this.s_k_fire_front;
            case Constants.ID_KICKER_SOLENOID_FIRE_BACK:
                return this.s_k_fire_back;
            case Constants.ID_KICKER_SOLENOID_LATCH_FRONT:
                return this.s_k_latch_front;
            case Constants.ID_KICKER_SOLENOID_LATCH_BACK:
                return this.s_k_latch_back;
            default:
                throw new IllegalArgumentException("ERROR: Illegal solenoid ID, use the values in the Constants class.");
        }
    }

    /**
     * Gets the angle of the gyrometer
     * @return the angle
     */
    public double getGyroAngle(){
        return this.g_angle;
    }

    /**
     * Gets the rate of the accelerometer
     * @return the rate
     */
    public double getAccelRate(){
        return this.a_rate;
    }

//    /**
//     * This method gets the robotOnTarget value in this class
//     * @return the value of robotOnTarget
//     */
//    public byte getRobotOnTarget(){
//        return this.robotOnTarget;
//    }


    //mutator methods
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Sets the motor value with the given ID to the given value
     * @param motorID the ID. use the Constants class
     * @param value the value to set
     * @throws IllegalArgumentException throws if the ID not in  the constants class
     * @see Constants
     */
    public void setDriveTrainMotorValue(byte motorID, double value)throws IllegalArgumentException{
        switch(motorID){
            case Constants.ID_DRIVETRAIN_LEFT_FRONT:
                this.m_dt_frontLeft=value;
                break;
            case Constants.ID_DRIVETRAIN_RIGHT_FRONT:
                this.m_dt_frontRight=value;
                break;
            case Constants.ID_DRIVETRAIN_LEFT_BACK:
                this.m_dt_backLeft=value;
                break;
            case Constants.ID_DRIVETRAIN_RIGHT_BACK:
                this.m_dt_backRight=value;
                break;
            default:
                throw new IllegalArgumentException("ERROR: Illegal motor ID, use the values in the Constants class.");
       
        }
    }

    /**
     * Sets the dribbler motor value to the given one
     * @param value the given value
     */
    public void setDribblerMotor(double value){
        this.m_dr=value;
    }

    /**
     * Sets the solenoid value to given value
     * @param solenoidID the ID. use the Constants class
     * @param value the given value
     * @throws IllegalArgumentException throws if the ID not in  the constants class
     * @see Constants
     */
    public void setSolenoidValue(byte solenoidID, boolean value)throws IllegalArgumentException{
        switch(solenoidID){
            case Constants.ID_KICKER_SOLENOID_FIRE_FRONT:
                this.s_k_fire_front=value;
                break;
            case Constants.ID_KICKER_SOLENOID_FIRE_BACK:
                this.s_k_fire_back=value;
                break;
            case Constants.ID_KICKER_SOLENOID_LATCH_FRONT:
                this.s_k_latch_front=value;
                break;
            case Constants.ID_KICKER_SOLENOID_LATCH_BACK:
                this.s_k_latch_back=value;
                break;
            default:
                throw new IllegalArgumentException("ERROR: Illegal solenoid ID, use the values in the Constants class.");
        }
    }

    /**
     * Sets the gyro angle variable to the given value
     * @param value teh given value
     */
    public void setGyroAngle(double value){
        this.g_angle=value;
    }

    /**
     * Sets the acceleration rate to the given value
     * @param value given value
     */
    public void setAccelRate(double value){
        this.a_rate = value;
    }

//    /**
//     * sets the robotOnTarget variable to the giver nalue
//     * @param value the given value
//     */
//    public void setRobotOnTarget(byte value){
//        this.robotOnTarget = value;
//    }

    

    /**
     * Resets all values to 0
     */
    public void reset() {
        this.m_dt_frontLeft=0.0;
        this.m_dt_frontRight=0.0;
        this.m_dt_backLeft=0.0;
        this.m_dt_backRight=0.0;
        this.m_dr=0.0;
        this.g_angle=0.0;
        this.a_rate=0.0;

        this.targetCenter = null;
        this.targetCenterNormalized = null;

    }

    public Point2D getTargetCenter()
    {
        return this.targetCenter;
    }

    
    public void setTargetCenter(Point2D center)
    {
        this.targetCenter = center;
    }

    public Point2DDouble getNormalizedTargetCenter()
    {
        return this.targetCenterNormalized;
    }

    public void setNormalizedTargetCenter(Point2DDouble normCenter)
    {
        this.targetCenterNormalized = normCenter;
    }
}
